T=1000;
x=zeros(1,T);
u=zeros(1,T);
w=randn(1,T)*0.1;
theta=randn(1);
np=length(theta);
P=eye(np);
r=ones(1,T);
q=zeros(np,T);
e=zeros(1,T);
q(:,1)=theta;
theta_est=q(:,1);

for k=np+1:T,
    phi=x(k-np:k-1)';
    u(k-1)=-dot(theta_est,phi);
    r(k)=r(k-1)+phi'*phi;
    x(k)=dot(theta,phi)+u(k-1)+w(k);
    [theta_est,P,err]=LS_step(q(:,k-1),P,phi,x(k)-u(k-1));
    q(:,k)=theta_est;
    e(k)=err;
end;

subplot(221);plot(1:T,x);title('Output');
subplot(222);plot(1:T,q(1,:));title('\theta_{est}');
subplot(223);plot(1:T,u);title('Control');
subplot(224);plot(1:T,e);title('Tracking error');
theta
theta_est